{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# Gal3\n",
        "\n",
        "The `Gal3` class implements two related but distinct concepts: a kinematic state on a manifold, and the Galilean group of spacetime transformations SGal(3). Unlike a `Pose3` which describes a static pose (position and orientation), a `Gal3` state describes a full kinematic state, including velocity and time. It is a 10-dimensional manifold. For users familiar with `NavState`, which also models attitude, position, and velocity, `Gal3` is different in that it forms a complete Lie group, with a well-defined composition rule that correctly propagates states through time."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Gal3.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "# Install GTSAM and Plotly from pip if running in Google Colab\n",
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass # Not in Colab"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import Gal3, Rot3, Point3, Event\n",
        "import numpy as np"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## `Gal3` as a Manifold\n",
        "\n",
        "First, we can view `Gal3` as a manifold representing the kinematic state of an object. An element on this manifold is a tuple $(C, p, v, t)$ where:\n",
        "- $C \\in SO(3)$: The **attitude** (orientation).\n",
        "- $p \\in \\mathbb{R}^3$: The **position**.\n",
        "- $v \\in \\mathbb{R}^3$: The **velocity**.\n",
        "- $t \\in \\mathbb{R}$: The **time**.\n",
        "\n",
        "This is useful for representing the state of a dynamic system in estimation problems like trajectory optimization."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Identity State:\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: 0 0 0\n",
            "v: 0 0 0\n",
            "t: 0\n",
            "\n",
            "\n",
            "Kinematic State G1:\n",
            "R: [\n",
            "\t0.866025, -0.5, 0;\n",
            "\t0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: 10 20 30\n",
            "v: 1 2 3\n",
            "t: 5\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Identity element (state at the origin)\n",
        "g_identity = gtsam.Gal3()\n",
        "print(f\"Identity State:\\n{g_identity}\\n\")\n",
        "\n",
        "# A custom kinematic state\n",
        "attitude = Rot3.Yaw(np.pi / 6)\n",
        "position = Point3(10, 20, 30)\n",
        "velocity = np.array([1, 2, 3])\n",
        "time = 5.0\n",
        "G1 = Gal3(attitude, position, velocity, time)\n",
        "print(f\"Kinematic State G1:\\n{G1}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "The state's components can be accessed using accessor methods that align with this manifold view."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Attitude:\n",
            "R: [\n",
            "\t0.866025, -0.5, 0;\n",
            "\t0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "\n",
            "Position: [10. 20. 30.]\n",
            "Velocity: [1. 2. 3.]\n",
            "Time: 5.0\n"
          ]
        }
      ],
      "source": [
        "print(f\"Attitude:\\n{G1.attitude()}\\n\")\n",
        "print(f\"Position: {G1.position()}\")\n",
        "print(f\"Velocity: {G1.velocity()}\")\n",
        "print(f\"Time: {G1.time()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## The Lie Group SGal(3)\n",
        "\n",
        "The `Gal3` class also implements the 3D Galilean group of spacetime transformations. When viewed as a transform, an element $g=(R, r, v, t)$ maps spacetime coordinates from one frame to another. In this context, we often refer to the components as rotation and translation, and `Gal3` provides aliases for this purpose:\n",
        "- `rotation()` is an alias for `attitude()`.\n",
        "- `translation()` is an alias for `position()`.\n",
        "\n",
        "The group operation defines how to compose two such transformations. For two transforms $g_1=(R_1, r_1, v_1, t_1)$ and $g_2=(R_2, r_2, v_2, t_2)$, their composition $g_{comp} = g_1 \\cdot g_2$ is:\n",
        "\n",
        "$$ R_{comp} = R_1 R_2 $$\n",
        "$$ v_{comp} = R_1 v_2 + v_1 $$\n",
        "$$ r_{comp} = R_1 r_2 + t_2 v_1 + r_1 $$\n",
        "$$ t_{comp} = t_1 + t_2 $$\n",
        "\n",
        "This composition rule correctly integrates the kinematic states."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "G1.inverse():\n",
            "R: [\n",
            "\t0.866025, 0.5, 0;\n",
            "\t-0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: -9.33013 -6.16025      -15\n",
            "v: -1.86603 -1.23205       -3\n",
            "t: -5\n",
            "\n",
            "\n",
            "G1 * G2:\n",
            "R: [\n",
            "\t0.707107, -0.707107, 0;\n",
            "\t0.707107, 0.707107, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: 13.8301 30.8301      41\n",
            "v:  3.4641 5.73205       8\n",
            "t: 7\n",
            "\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Create a second transform\n",
        "G2 = Gal3(Rot3.Yaw(np.pi / 12), Point3(5, 5, 5), np.array([4, 2, 5]), 2.0)\n",
        "\n",
        "# Inverse\n",
        "g_inv = G1.inverse()\n",
        "print(f\"G1.inverse():\\n{g_inv}\\n\")\n",
        "\n",
        "# Composition\n",
        "g_composed = G1 * G2\n",
        "print(f\"G1 * G2:\\n{g_composed}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Group Action on Spacetime Events\n",
        "\n",
        "The natural action for a Galilean transform $g=(R,r,v,t)$ is on a spacetime `Event`, which is a point in space and time $e=(p_{in}, t_{in})$. The `act` method transforms an event into a new reference frame. The new event $(p_{out}, t_{out})$ is calculated as:\n",
        "\n",
        "$$ t_{out} = t + t_{in} $$\n",
        "$$ p_{out} = R \\cdot p_{in} + v \\cdot t_{in} + r $$\n",
        "\n",
        "This action describes how the coordinates of a spacetime event change under the Galilean transformation."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Transformed Event:\n",
            "{'time':15, 'location': 19.7321 44.4641      66}\n"
          ]
        }
      ],
      "source": [
        "# Create a spacetime event\n",
        "event = Event(10.0, Point3(2, 4, 6))\n",
        "\n",
        "# Apply the Gal3 transform to the event\n",
        "transformed_event = G1.act(event)\n",
        "print(f\"Transformed Event:\\n{transformed_event}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Lie Algebra and Manifold Operations\n",
        "\n",
        "The Lie algebra of SGal(3), `sgal(3)`, is the 10D tangent space at the identity. A tangent vector `xi` is a 10D vector `xi = [ρ, ν, θ, τ]` where `ρ` (displacement), `ν` (velocity change), `θ` (rotation vector) are 3D vectors and `τ` is a scalar time change. The `Expmap` and `Logmap` functions convert between this Lie algebra representation and the `Gal3` group element.\n",
        "\n",
        "As with other Lie groups in GTSAM, `Gal3` is a manifold, and the `retract` and `localCoordinates` methods are used for optimization. By default, they are implemented using the group's `Expmap` and `Logmap`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Expmap(xi):\n",
            "R: [\n",
            "\t0.935755, -0.283165, 0.210192;\n",
            "\t0.302933, 0.950581, -0.0680313;\n",
            "\t-0.18054, 0.127335, 0.97529\n",
            "]\n",
            "r: 0.236809 0.242157 0.299625\n",
            "v: 0.381202 0.528659  0.58716\n",
            "t: 0.7\n",
            "\n",
            "\n",
            "Logmap(G_exp) = [0.1  0.2  0.3  0.4  0.5  0.6  0.1  0.05 0.1  0.7 ]\n"
          ]
        }
      ],
      "source": [
        "# Create a vector in the Lie algebra [rho, nu, theta, tau]\n",
        "xi = np.array([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.1, 0.05, 0.1, 0.7])\n",
        "\n",
        "# Exponential map: from Lie algebra to group\n",
        "G_exp = Gal3.Expmap(xi)\n",
        "print(f\"Expmap(xi):\\n{G_exp}\\n\")\n",
        "\n",
        "# Logarithm map: from group to Lie algebra\n",
        "xi_log = Gal3.Logmap(G_exp)\n",
        "print(f\"Logmap(G_exp) = {np.round(xi_log, 4)}\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "localCoordinates(q) from p = [  0.      0.     -0.262   3.608  -0.24    3.     -9.429  -1.365 -15.5\n",
            "  -3.   ]\n",
            "\n",
            "p.retract(v):\n",
            "R: [\n",
            "\t0.866025, -0.5, 0;\n",
            "\t0.5, 0.866025, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: 10 20 30\n",
            "v: 1 2 3\n",
            "t: 5\n",
            "\n"
          ]
        }
      ],
      "source": [
        "p = Gal3(Rot3.Yaw(np.pi / 4), Point3(15, 30, 50), np.array([-2, 0, 0]), 8.0)\n",
        "q = G1 # from the first example\n",
        "\n",
        "# Find the tangent vector to go from p to q\n",
        "v = p.localCoordinates(q)\n",
        "print(f\"localCoordinates(q) from p = {np.round(v, 3)}\\n\")\n",
        "\n",
        "# Move from p along v to get back to q\n",
        "q_retracted = p.retract(v)\n",
        "print(f\"p.retract(v):\\n{q_retracted}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}